/*
===========================================================================
This source file is part of DyLab (Dynamic Laboratory)
For the latest info, see http://dylab.googlecode.com

Copyright (c) 2006-2008 Lukas Krejci
(krejci.lukas@volny.cz)

This file is part of DyLab.

    DyLab is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    DyLab is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with DyLab.  If not, see <http://www.gnu.org/licenses/>.
===========================================================================
*/

//===========================================================================================
#include <dylab/utils/geometry/TriangleGeometry3.h>

//===========================================================================================
void dylab::TriangleGeometry3::getEdgeVertexIndices(edgeidx_t _edgeIdx, 
	vertexidx_t (&_vertexIndices)[EdgeGeometry3::V_COUNT])
{
    switch (_edgeIdx)
    {
    case E_0: 
        _vertexIndices[EdgeGeometry3::V_0] =  V_0,
        _vertexIndices[EdgeGeometry3::V_1] =  V_1; 
        break;
    case E_1: 
        _vertexIndices[EdgeGeometry3::V_0] =  V_1,
        _vertexIndices[EdgeGeometry3::V_1] =  V_2; 
        break;
    case E_2: 
        _vertexIndices[EdgeGeometry3::V_0] =  V_2,
        _vertexIndices[EdgeGeometry3::V_1] =  V_0; 
        break;
    default: 
		throw EInvalidArgument(String::fmt("Invalid edge index: %1%", static_cast<uint32_t>(_edgeIdx)));
    }
}
//-----------------------------------------------------------------------------

//===========================================================================================
dylab::Vector3 dylab::TriangleGeometry3::computeBarycentricCoords(const Vector3 & _p) const
{                    
    // compute area of the triangle
    real_t area = computeArea();

    // shouldn't be zero
    if (area == Math::ZERO)
        throw ENumericError(String::fmt("Can't compute barycentric coordinates for point %1% inside the triangle %2% (degenerated: %3%)", _p, *this, isDegenerated()));

    // compute & return the barycentric coordinates
    Vector3 bc;
    bc.y() = TriangleGeometry3(mVertices[V_0], mVertices[V_2], _p).computeArea() / area;
    bc.z() = TriangleGeometry3(mVertices[V_0], mVertices[V_1], _p).computeArea() / area;
    bc.x() = Math::ONE - bc.y() - bc.z();
    return bc;
}
//-----------------------------------------------------------------------------
dylab::TriangleGeometry3::pointcls_t 
dylab::TriangleGeometry3::classifyPoint(const Vector3 & _p) const
{
    // compute barycentric coordinates of the point _p inside the triangle
    Vector3 bc = computeBarycentricCoords(_p);

    // store the zero flag of all coords
    bool xAlmostZero = Math::almostZero(bc.x(), DYLAB_R(100) * Math::EQUALITY_TOLERANCE);
    bool yAlmostZero = Math::almostZero(bc.y(), DYLAB_R(100) * Math::EQUALITY_TOLERANCE);
    bool zAlmostZero = Math::almostZero(bc.z(), DYLAB_R(100) * Math::EQUALITY_TOLERANCE);

    // first test, whether the point is not outside
    real_t bc2sum = bc.y() + bc.z();
    if ((((bc.y() > Math::ZERO) || yAlmostZero) && ((bc.z() > Math::ZERO) || zAlmostZero)) && 
          (bc2sum < Math::ONE || Math::almostEqual(bc2sum, Math::ONE)))
    {
        // test, if point is one of triangle's vertices
        if (yAlmostZero && zAlmostZero)                                
            return pointcls_t(Math::POINTCLS_VERTEX, V_0);
        if (xAlmostZero && zAlmostZero)
            return pointcls_t(Math::POINTCLS_VERTEX, V_1);
        if (xAlmostZero && yAlmostZero)
            return pointcls_t(Math::POINTCLS_VERTEX, V_2);

        // test, if point lies in one of triangle's edges
        if (zAlmostZero)
            return pointcls_t(Math::POINTCLS_EDGE, E_0);
        if (xAlmostZero)
            return pointcls_t(Math::POINTCLS_EDGE, E_1);
        if (yAlmostZero)
            return pointcls_t(Math::POINTCLS_EDGE, E_2);

        // _p is inner point
        return pointcls_t(Math::POINTCLS_INNER, FeatureIdx());
    }
    else
    {
        // the point is outside
        return pointcls_t(Math::POINTCLS_OUTER, FeatureIdx());
    }
}
//-----------------------------------------------------------------------------
dylab::Math::intersection_t 
dylab::TriangleGeometry3::computeLineIntersection(const Vector3 & _a, const Vector3 & _b, 
	Vector3 & _ip, bool allowAMinus, bool allowBPlus) const
{
    Vector3    u, v, n;         // triangle vectors
    Vector3    w0, w;           // ray vectors
    real_t     r, a, b;         // params to calc ray-plane intersect                    

    // get triangle edge vectors and plane normal
    u = mVertices[1] - mVertices[0];
    v = mVertices[2] - mVertices[0];
    n = u.cross(v);  

    // test, if triangle is degenerate
    if (n.almostZero())         
        return Math::INTERSECTION_NONE;  

    // compute triangle plane intersection
    Vector3 dir = _b - _a;
    w0 = _a - mVertices[0];
    a = - n.dot(w0);
    b = n.dot(dir);

    // test, if ray is parallel to triangle plane
    if (b == 0) 
    {      
        // test, if ray lies in triangle plane
        if (a == 0)                 
            return Math::INTERSECTION_LINE;                        
        else // ray disjoint from plane
            return Math::INTERSECTION_NONE;           
    }

    // get intersect point of ray with triangle plane
    r = a / b;

    // test, if ray goes away from triangle, before point _a
    if (r < Math::ZERO && !allowAMinus)        
        return Math::INTERSECTION_NONE;                          

    // test, ray goes after point _b
    if (r > Math::ONE && !allowBPlus)         
        return Math::INTERSECTION_NONE;                          

    // intersect point of ray and plane
    _ip = _a + dir * r;         

    // is _ip inside Triangle?
    real_t uu, uv, vv, wu, wv, D;
    uu = u.dot(u);
    uv = u.dot(v);
    vv = v.dot(v);
    w = _ip - mVertices[0];
    wu = w.dot(u);
    wv = w.dot(v);
    D = uv * uv - uu * vv;

    // get and test parametric coords
    real_t s, t;

    // test 1, if _ip is outside Triangle
    s = (uv * wv - vv * wu) / D;
    if (s < Math::ZERO || s > Math::ONE)       
        return Math::INTERSECTION_NONE;
 
    // test 2, _ip is outside Triangle
    t = (uv * wu - uu * wv) / D;    
    if (t < Math::ZERO || (s + t) > Math::ONE) 
        return Math::INTERSECTION_NONE;

    // _ip is in Triangle
    return Math::INTERSECTION_POINT;    
}
//-----------------------------------------------------------------------------

//===========================================================================================
std::ostream & dylab::operator << (std::ostream & _os, const TriangleGeometry3::pointcls_t & _pointCls)
{ 
    switch (_pointCls.first)
    {
	case Math::POINTCLS_INNER: 
        return _os << "POINTCLS_INNER"; 
    case Math::POINTCLS_VERTEX: 
		return _os << String::fmt("POINTCLS_VERTEX(V_%1%)", _pointCls.second.vertexIdx); 
    case Math::POINTCLS_EDGE: 
        return _os << String::fmt("POINTCLS_EDGE(E_%1%)", _pointCls.second.edgeIdx); 
    case Math::POINTCLS_OUTER: 
        return _os << "POINTCLS_OUTER"; 
    default: 
        throw EInvalidArgument(String::fmt("Invalid point-triangle classification: %1%", _pointCls.first));
        return _os;
    }
}
//-----------------------------------------------------------------------------